/***************************************************************************
 创建者: 华磊
 开始时间: 2018.9.26
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file .h
 *  功能: V槽探测功能
 *
 *                                                                         *
 ***************************************************************************/

#ifndef VISIONMATCH_H
#define VISIONMATCH_H
#include <qstring.h>
#include <vector>
#include "GeneralDefine.h"
#include "qmap.h"

namespace cv{
class Mat;
}

namespace KDL{
class Frame;
}

class LineMatch;
class FilteringAlgorithm;

enum E_DETECT_FAILED_TYPE
{
    E_DETECT_FAILED_TYPE_NONE,
    E_DETECT_FAILED_TYPE_L1_ZERO,
    E_DETECT_FAILED_TYPE_L2_ZERO,
    E_DETECT_FAILED_TYPE_GAP_NOT_GOOD,
    E_DETECT_FAILED_TYPE_ANGLE_NOT_GOOD,
    E_DETECT_FAILED_TYPE_TOO_MANY_GOOD,
};


class KalmanFilterSimple;

class VisionMatch
{
public:
    VisionMatch(QString configFileIn, int robotIdIn,int laserSkipCountIn);

public:
    int getVmarkPositionCertainity(VehiclePosition &uncertainPoseOut);//算法用于暂停和报错。
    int setWheelOdometerData_noRos( struct OdemeterData odemeterDataIn);
    int setKalmanFilterPara(int filterId, double followQ, double observeR);
    int setLineFittingMethod(int methodIn);
    int setIsPrintVmarkProcessData(bool isPrint);
    int laserFilter_test(const LaserDataInfo &frontLaserDataIn,
                                               LaserDataInfo &frontLaserDataOut);
    bool isLaserPointOk_region_test(double x_in,double y_in);
    int getVMarkPositionInWorld(VehiclePosition &positionOut,bool &isSucess);//todo
    int setReferenceBaselinkPosition(VehiclePosition positionIn);
    int setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn,
                                double maxAngleIn);
    int setLaserIntensityFilterByType(int typeIn);
    int setLaserSensorData(const LaserDataInfo *laserData);
    /**
     * @brief detectReflectLine 使用傲视雷达测试，一睹２米长的墙，运算时间消耗１１～４７ｓ,长时间会导致里程计位置反馈
     * 很卡顿，然后先减速后猛冲报错，查看反馈波形图，也不够连续平滑，卡顿。
     * @param laserData
     * @param x_out
     * @param y_out
     * @param r_out
     * @return
     */
    int detectReflectLine(PositionInfo &vehiclePositionInfoOut);
    /**
     * @brief calculateVMarkPosition_byLaser
     * @param baseLink_x unit is m
     * @param baseLink_y unit is m
     * @param baseLink_r unit is rad
     * @param x_out unit is m
     * @param y_out unit is m
     * @param r_out  unit is rad
     * @return
     */
//    int calculateVMarkPosition_byLaser(double baseLink_x,double baseLink_y,double baseLink_r,
//                                       vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    /**
     * @brief setLaserLinkInBaseLink
     * @param x unit is m
     * @param y
     * @param z
     * @param roll unit is rad
     * @param pitch
     * @param yaw
     * @return
     */
    int setLaserLinkInBaseLink(double x,double y,double z,double roll,double pitch,double yaw);
    /**
     * @brief getTimeConsumed
     * @param minConsumedOut unit is us
     * @param currentConsumedOut  unit is us
     * @param maxConsumedOut  unit is us
     * @return
     */
    int getTimeConsumed(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int resetRosMeasureTime();
    int setCurrentVMarkIndex(int indexIn);
    int setCurrentVMarkType(int currentVmarkTypeIn);
    int setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn);
    int getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut);
    int updateVMarkFile(QString vmarkFileIn);
    int createVMark(VMarkInfo infoIn,QString vmarkFileIn);
    /**
     * @brief detectVMark_baseMap　求地图上ｖmark的位置
     * @param min_angle unit is degree
     * @param max_angle unit is degree
     * @param max_distance 交点离直线端点的最大距离 unit m
     * @param allow_vDistance　多个vmark之间的距离 unit m
     * @param allow_vAngle　多个vmark之间的角度 unit degree
     * @param x_out　ｖmark　的交点 unit m world坐标系
     * @param y_out　ｖmark　的交点 unit m　world坐标系
     * @param r_out　ｖmark　的角度 unit rad
     * @return
     */
    int detectVMark_baseMap(double min_angle, double max_angle, double max_distance, double allow_vDistance, double allow_vAngle,
                            vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int detectVMark_baseMap( vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    /**
     * @brief detectVMark_laser 根据激光数据，计算车base_link的位置
     * @param x_out 车base_link 在world坐标系ｘ unit m
     * @param y_out 车base_link 在world坐标系y unit m
     * @param r_out  unit rad
     * @return
     */
//    int detectVMark_laser(int vmark_index, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
//    int detectVMark_laser(vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);

    int setMatchCalculateParameter(MatchCalculationParameter paraIn);
    int getMatchDebugInfo(VisionMatchDebugInfo &infoOut);

    /**
     * @brief loadBaseMap  加载地图的过程中，不允许调用计算，可能出错。
     * @param mapPath
     * @return
     */
//    int loadBaseMap(QString mapPath,double mPerPixIn,double x_offset,double y_offset,double r_offset);
    int setBaseMapRegion_m(double originX,double originY, double width,double height);

    /**
     * @brief loadLocalMap
     * @param pointX , unit is m 经过变换后，以车laser_link为原点
     * @param pointY, unit is m　经过变换后，以车laser_link为原点
     * @return
     */
    int loadLocalMap(std::vector<double> pointX, std::vector<double> pointY);


    /**
     * @brief setLocalMapRegion_m
     * @param originX 相对于laserscan原点
     * @param originY
     * @param width
     * @param height
     * @return
     */
    int setLocalMapRegion_m(double originX,double originY, double width,double height);


    /**
     * @brief getLaserPositionCm　相对于baseMap的原始原点（非图像原点）
     * @param xOut,unit is m
     * @param yOut,unit is m
     * @param rOut,unit is rad
     * @param isCalculateSucess_out
     * @return
     */
    int getLaserPosition_m(double &xOut,double &yOut,double &rOut,bool &isCalculateSucess_out);
    int getVehiclePosition_m(double &xOut,double &yOut,double &rOut,bool &isCalculateSucess_out);

    //------for debug
    int showDebugWindow(int id);
    int showBaseMap();
    int showBaseCompareMap();
    int showLocalMap();
    int showLocalCampareMap();
    int showMatchWindow();

private:
    double angleRotationJudgement(double targetAngle, double initialAngle);
    int calculateAgvPositionProbolityError(VehiclePosition oldPoseIn,VehiclePosition newPoseIn,
                                           VehiclePosition &poseErrorInfoOut);//计算AGV可能误差。与里程和时间关联，校准后复位。

    int calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut);
    int calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                              KDL::Frame *frame_odom_in_mapOut);
    int filterPositionResult(double &pose_x, double &pose_y, double &pose_r);
    int transBaseLinkPointToLaserPoint(double x_in,double y_in,double &x_out,double &y_out);
    int translateToRegion(double front,double back,double left,double right,
                          std::vector<double> &region_x, std::vector<double> &region_y);
    int calculateVmarkPositionInWorld(VehiclePosition vmarkInLaser,VehiclePosition &vmarkInWorldOut);
    int translate_structLine_in_laserlink_2_baselink_in_structLine(double xIn, double yIn, double rIn, int method,
                                                               double &xOut, double &yOut, double &rOut);
    int translate_vmark_in_localCompareMap_2_baselink_in_world(double xIn, double yIn, double rIn, int vmarkIndexIn,
                                                               double &xOut, double &yOut, double &rOut);
    int translate_vmark_in_localCompareMap_2_vmark_in_world(double xIn,double yIn,double rIn,
                                                            double x_baselink,double y_baselink,double r_baselink,
                                                               double &xOut,double &yOut,double &rOut);
    int calculateMatch();
    int calculateMatch(double &xOut,double &yOut,double &rOut,bool &isCalculateSucess_out);
//    int setLaserToVehicleInfo(LaserScanDeviceInfo infoIn);
//    int setBaseMapRegion_pix(double originX,double originY, double width,double height);
    /**
     * @brief setLocalMapRegion_pix
     * @param originX 相对于localMap图片的ｐｉｘ值
     * @param originY
     * @param width
     * @param height
     * @return
     */
//    int setLocalMapRegion_pix(double originX,double originY, double width,double height);
    /**
     * @brief getLaserPositionPix 相对于baseMap的原始原点（非图像原点）
     * @param xOut ,unit is pix
     * @param yOut,unit is pix
     * @param rOut,unit is rad
     * @param isCalculateSucess_out
     * @return
     */
    int getLaserPosition_pix(double &xOut,double &yOut,double &rOut,bool &isCalculateSucess_out);
    int loadLocalMap(QString mapPath);

    int readVMarkConfigFile(QString filePathIn, QMap<int, VMarkDetectParameter> &vmark_detect_Parameter_out);
    int readStructLineConfigFile(QString filePathIn, QMap<int, StructLineParameter> &structLine_Parameter_out, QMap<int, LaserFilterParameter> &laserFilter_ParameterOut);
    int readVMarkFile(QString filePathIn,QMap<int,VMarkInfo> &vmarkPositionOut);
    int translateBaseMapImagePointToWorldPoint(vector<double> x_in,vector<double> y_in,vector<double> r_in,
                                      vector<double> &x_out,vector<double> &y_out,vector<double> &r_out);
    int translateBaseMapImagePointToWorldPoint(double x_in,double y_in,double r_in,double &x_out,double &y_out,double &r_out);
    int translateWorldPointToBaseMapImagePoint(double x_in,double y_in,double r_in,double &x_out,double &y_out,double &r_out);

//    /**
//     * @brief setLaserPosInLocalMap 设置激光原点在局部地图的位置
//     * @param laserX_in //pix
//     * @param laserY_in //pix
//     * @param laserR_in //rad
//     * @return
//     */
//    int setLaserPosInLocalMap_pix( double laserX_in,double laserY_in,double laserR_in);

//    int calculateMatch_surf();
//    int calculateMatch_surfLowe();
//    int calculateMatch_sift();
//    int calculateMatch_siftLowe();//good，速度慢，噪音敏感
//    int calculateMatch_orb();//速度快，综合性能好。
//    int calculateMatch_fast();//基于角点，速度快
//    int calculateMatch_harris();//基于角点.
//    int calculateMatch_vmark();//基于vmark
    //other calculate :HOG、Harr、LBP. reference :https://www.cnblogs.com/skyfsm/p/7401523.html

    int transPointToBaseMap();
    int getBaseMapMatch(double &xOut,double &yOut,double &rOut);
    int imagine_threshold_process( cv::Mat* imagine );
    int processPictureForVMark(const cv::Mat &originPicture, cv::Mat &transformedPicture);
    double transformToPi(double angleIn);

    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0);


private:
    LaserDataInfo laserData_received;
    int laserDateReadyCount;
    cv::Mat* baseMap_originSource;//基础地图_原始
    cv::Mat* baseMap;//基础地图
    double baseMap_originX;//pix 基础地图中的world原点ｘ
    double baseMap_originY;//pix　基础地图中的world原点ｙ
    double baseMap_originR;//current not use

    cv::Mat* baseCompareMap;//基础比较地图
    double baseCompare_originX;//pix 基础比较地图中的world原点ｘ
    double baseCompare_originY;//pix　基础比较地图中的world原点ｙ
    double baseCompare_width;//pix 基础比较地图宽
    double baseCompare_height;//pix　基础比较地图高

    cv::Mat* localMap;//局部地图
    double localMap_originX;//pix　laser_link原点在局部地图的ｘ
    double localMap_originY;//pix laser_link原点在局部地图的y
    double localMap_originR;

    cv::Mat* localCompareMap;//局部比较地图
    double localCompare_originX;//pix　laser_link原点在局部比较地图的ｘ
    double localCompare_originY;//pix　laser_link原点在局部比较地图的y
    double localCompare_width;//pix
    double localCompare_height;//pix


    int local_matchX;//pix
    int local_matchY;//pix
    double local_matchR;

    bool isMatchSucess;

    cv::Mat* transMat;
    double mPerPix;//分辨率

    pthread_mutex_t matchMutex;
    pthread_mutexattr_t matchAttr;

    KDL::Frame *frame_baselink_in_laserlink;
    KDL::Frame *frame_laserlink_in_baselink;

    VisionMatchDebugInfo matchDebugInfo;

    MatchCalculationParameter calculationParemeter;
    LineMatch* lineMatch;
    QMap<int,VMarkInfo> vmarkPosition;

    QMap<int,VMarkDetectParameter> vmark_detect_Parameter;
    QMap<int,StructLineParameter> structLine_detect_Parameter;
    QMap<int,LaserFilterParameter> laserFilter_Parameter;
    QString configFile;
    int robotId;
    int laserSkipCount;
    int currentVmarkIndex;//not used
    int currentVmarkType;

    double minConsumed;
    double currentConsumed;
    double maxConsumed;

    double laser_baslink_pitch;
    VehiclePosition baselinkReferencePosition;
    PositionInfo vmarkInWorldPosition;

//    StructLineParameter lineParameter;
    E_DETECT_FAILED_TYPE detectFailedType;
    bool isPrintVmarkProcessData;

    FilteringAlgorithm* filtering_vmarkPosition_x;
    FilteringAlgorithm* filtering_vmarkPosition_y;
    FilteringAlgorithm* filtering_vmarkPosition_r;
    KalmanFilterSimple *kalmanFilter;


    OdemeterData odemeterData;//里程计信息
    KDL::Frame *frame_baselink_in_odom;//轮子里程计
    KDL::Frame *frame_odom_in_map;//里程计偏差

    PositionInfo agvInMapPosition;//初始位置，校正位置
    PositionInfo agvInMapPosition_old;
    VehiclePosition laserlinkInMapPosition;//激光雷达初始位置，校正位置
    VehiclePosition poseErrorInfo;//位置误差
    double odomErrorRatio_angle;
    double odomErrorRatio_position;
};

#endif // VISIONMATCH_H
